/*
 * @Author: FamiliennameistChow nameistchow@hotmail.com
 * @Date: 2023-11-02 23:11:35
 * @LastEditors: FamiliennameistChow nameistchow@hotmail.com
 * @LastEditTime: 2023-11-07 22:14:25
 * @FilePath: /pcl_tutorials/PointCloudRegistration.hpp
 * @Description: 
 * 
 * Copyright (c) 2023 by FamiliennameistChow, All Rights Reserved. 
 */
#ifndef _POINT_CLOUD_REG_H_
#define _POINT_CLOUD_REG_H_

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/registration/icp.h>
#include <iostream>

typedef pcl::PointXYZI PointType;

class PointCloudRegistration
{
private:
    /* data */
public:
    PointCloudRegistration(/* args */){};
    ~PointCloudRegistration(){};

	// point to point icp method
	static bool icp(pcl::PointCloud<PointType>::Ptr &cloud_source, pcl::PointCloud<PointType>::Ptr &cloud_target){
        
		pcl::IterativeClosestPoint<PointType, PointType> icp;

		icp.setMaxCorrespondenceDistance(0.05);
		icp.setEuclideanFitnessEpsilon(0.0001); 
		icp.setMaximumIterations(50); 
		icp.setRANSACIterations(50);
		icp.setRANSACOutlierRejectionThreshold(0.05);
		icp.setTransformationEpsilon(1e-8);
		//icp.setEuclideanFitnessEpsilon(1);
		icp.setInputSource(cloud_source);
		icp.setInputTarget(cloud_target);

		icp.align(*cloud_source);

		if (icp.hasConverged() == false){
			std::cout << " icp have not converged !!!!! " << std::endl;
			return false;
		}

		double score = icp.getFitnessScore();
		std::cout << " icp score:  " << score << std::endl;
		Eigen::Matrix4f final_res = icp.getFinalTransformation();

		std::cout << " res pose: \n" << final_res << std::endl;
    }
};




#endif